﻿using System;

namespace BattleBotsMattAndSunny.Utilities
{
    public static class RoboMath
    {
        public static void Quadratic(double a, double b, double c, out double? x1, out double? x2)
        {
            var root = (b * b) - 4 * a * c;

            if (root < 0)
            {
                x1 = null;
                x2 = null;
            }
            else
            {
                var sqrt = Math.Sqrt(root);
                x1 = (-b + sqrt) / (2 * a);
                x2 = (-b - sqrt) / (2 * a);
            }
        }

        public static double GetDistance(double x1, double y1, double x2, double y2)
        {
            var deltaX = x1 - x2;
            var deltaY = y1 - y2;
            return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
        }

        public static double AbsoluteBearing(double x1, double y1, double x2, double y2)
        {
            // computes the absolute bearing between two points
            double xo = x2 - x1;
            double yo = y2 - y1;
            double hyp = GetDistance(x1, y1, x2, y2);
            double arcSin = (Math.Asin(xo / hyp)) * (57.2957795); //convert to degrees
            double bearing = 0;

            if (xo > 0 && yo > 0)
            { // both pos: lower-Left
                bearing = arcSin;
            }
            else if (xo < 0 && yo > 0)
            { // x neg, y pos: lower-right
                bearing = 360 + arcSin; // arcsin is negative here, actuall 360 - ang
            }
            else if (xo > 0 && yo < 0)
            { // x pos, y neg: upper-left
                bearing = 180 - arcSin;
            }
            else if (xo < 0 && yo < 0)
            { // both neg: upper-right
                bearing = 180 - arcSin; // arcsin is negative here, actually 180 + ang
            }

            return bearing;
        }
    }
}